#!/usr/bin/env python3
"""
增强版无人机导航系统模拟
包含PID控制器、MPC控制和A*路径规划
添加动态重规划功能
"""

import numpy as np
import matplotlib.pyplot as plt
from matplotlib.animation import FuncAnimation
import math
import time
from queue import PriorityQueue
import logging
from logging.handlers import RotatingFileHandler
import csv
import os
from datetime import datetime
from mpl_toolkits.mplot3d.art3d import Poly3DCollection
import random  # 添加随机模块用于模拟未知障碍物

# 设置支持中文的字体
plt.rcParams['font.sans-serif'] = ['SimHei']
plt.rcParams['axes.unicode_minus'] = False


# 设置日志记录
def setup_logging():
    if not os.path.exists('logs'):
        os.makedirs('logs')

    log_filename = f"logs/drone_navigation_{datetime.now().strftime('%Y%m%d_%H%M%S')}.log"
    log_format = '%(asctime)s - %(levelname)s - %(message)s'

    logging.basicConfig(
        level=logging.INFO,
        format=log_format,
        handlers=[
            RotatingFileHandler(log_filename, maxBytes=10 * 1024 * 1024, backupCount=5),
            logging.StreamHandler()
        ]
    )

    return log_filename


# 创建CSV数据记录器
class DataLogger:
    def __init__(self):
        if not os.path.exists('data'):
            os.makedirs('data')

        self.filename = f"data/drone_data_{datetime.now().strftime('%Y%m%d_%H%M%S')}.csv"

        with open(self.filename, 'w', newline='') as csvfile:
            writer = csv.writer(csvfile)
            writer.writerow([
                'timestamp', 'pos_x', 'pos_y', 'pos_z',
                'vel_x', 'vel_y', 'vel_z',
                'target_x', 'target_y', 'target_z',
                'error_x', 'error_y', 'error_z', 'total_error',
                'control_force_x', 'control_force_y', 'control_force_z',
                'replanning_events', 'obstacles_detected'
            ])

    def log_data(self, timestamp, position, velocity, target, control_force, replanning_events=0, obstacles_detected=0):
        error = target - position
        total_error = np.linalg.norm(error)

        with open(self.filename, 'a', newline='') as csvfile:
            writer = csv.writer(csvfile)
            writer.writerow([
                timestamp,
                position[0], position[1], position[2],
                velocity[0], velocity[1], velocity[2],
                target[0], target[1], target[2],
                error[0], error[1], error[2], total_error,
                control_force[0], control_force[1], control_force[2],
                replanning_events, obstacles_detected
            ])


class PIDController:
    """PID控制器实现"""

    def __init__(self, kp, ki, kd, max_output=float('inf')):
        self.kp = np.array(kp)
        self.ki = np.array(ki)
        self.kd = np.array(kd)
        self.max_output = max_output

        self.integral = np.zeros(3)
        self.previous_error = np.zeros(3)
        self.previous_time = time.time()

        self.p_term = np.zeros(3)
        self.i_term = np.zeros(3)
        self.d_term = np.zeros(3)

    def compute(self, setpoint, current_value):
        current_time = time.time()
        dt = current_time - self.previous_time
        if dt <= 0:
            return np.zeros(3)

        error = setpoint - current_value

        self.p_term = self.kp * error
        self.integral += error * dt

        max_integral = np.zeros(3)
        for i in range(3):
            if self.ki[i] != 0:
                max_integral[i] = 5.0 / self.ki[i]
            else:
                max_integral[i] = float('inf')

        self.integral = np.clip(self.integral, -max_integral, max_integral)
        self.i_term = self.ki * self.integral

        derivative = (error - self.previous_error) / dt
        self.d_term = self.kd * derivative

        output = self.p_term + self.i_term + self.d_term

        output_norm = np.linalg.norm(output)
        if output_norm > self.max_output:
            output = output / output_norm * self.max_output

        self.previous_error = error
        self.previous_time = current_time

        logging.debug(f"PID - P: {self.p_term}, I: {self.i_term}, D: {self.d_term}, Output: {output}")

        return output


class MPCController:
    """改进版模型预测控制器"""

    def __init__(self, horizon=5, dt=0.1):
        self.horizon = horizon
        self.dt = dt

        self.Q = np.diag([10, 10, 10])
        self.R = np.diag([0.1, 0.1, 0.1])
        self.Qf = np.diag([20, 20, 20])

        self.A = np.eye(3)
        self.B = np.eye(3) * dt

        self.last_control = np.zeros(3)
        self.last_error = np.zeros(3)

    def compute(self, current_state, reference_trajectory):
        current_pos = current_state[:3]
        current_vel = current_state[3:6]

        error = reference_trajectory[0] - current_pos

        Kp = 3.0
        Kd = 2.0

        control = Kp * error + Kd * (self.last_error - error) / self.dt

        control_norm = np.linalg.norm(control)
        if control_norm > 5.0:
            control = control / control_norm * 5.0

        self.last_error = error
        self.last_control = control

        logging.debug(f"MPC - Error: {error}, Control: {control}")

        return control


class AStarPlanner:
    """改进版A*路径规划算法实现"""

    def __init__(self, grid_size=1.0):
        self.grid_size = grid_size

    def heuristic(self, a, b):
        return math.sqrt((a[0] - b[0]) ** 2 + (a[1] - b[1]) ** 2 + (a[2] - b[2]) ** 2)

    def plan(self, start, goal, obstacles):
        start_grid = (int(round(start[0] / self.grid_size)),
                      int(round(start[1] / self.grid_size)),
                      int(round(start[2] / self.grid_size)))
        goal_grid = (int(round(goal[0] / self.grid_size)),
                     int(round(goal[1] / self.grid_size)),
                     int(round(goal[2] / self.grid_size)))

        obstacle_grid = set()
        for obs in obstacles:
            obs_grid = (int(round(obs[0] / self.grid_size)),
                        int(round(obs[1] / self.grid_size)),
                        int(round(obs[2] / self.grid_size)))
            obstacle_grid.add(obs_grid)

        frontier = PriorityQueue()
        frontier.put((0, start_grid))
        came_from = {}
        cost_so_far = {}
        came_from[start_grid] = None
        cost_so_far[start_grid] = 0

        directions = []
        for dx in [-1, 0, 1]:
            for dy in [-1, 0, 1]:
                for dz in [-1, 0, 1]:
                    if dx == 0 and dy == 0 and dz == 0:
                        continue
                    directions.append((dx, dy, dz))

        found_path = False
        while not frontier.empty():
            current = frontier.get()[1]

            if current == goal_grid:
                found_path = True
                break

            for dx, dy, dz in directions:
                next_node = (current[0] + dx, current[1] + dy, current[2] + dz)

                if next_node in obstacle_grid:
                    continue

                if abs(next_node[0]) > 50 or abs(next_node[1]) > 50 or next_node[2] < 0 or next_node[2] > 20:
                    continue

                new_cost = cost_so_far[current] + math.sqrt(dx * dx + dy * dy + dz * dz)
                if next_node not in cost_so_far or new_cost < cost_so_far[next_node]:
                    cost_so_far[next_node] = new_cost
                    priority = new_cost + self.heuristic(goal_grid, next_node)
                    frontier.put((priority, next_node))
                    came_from[next_node] = current

        if not found_path:
            logging.warning("A*路径规划失败，使用直线路径")
            return [start, goal]

        path = []
        current = goal_grid
        while current != start_grid:
            path.append(current)
            if current not in came_from:
                logging.error("A*路径重建失败")
                return [start, goal]
            current = came_from[current]
        path.append(start_grid)
        path.reverse()

        continuous_path = []
        for p in path:
            continuous_path.append(np.array([
                p[0] * self.grid_size,
                p[1] * self.grid_size,
                p[2] * self.grid_size
            ]))

        smoothed_path = self.smooth_path(continuous_path, obstacles)

        logging.info(f"A*规划完成，共{len(smoothed_path)}个航点")
        return smoothed_path

    def smooth_path(self, path, obstacles):
        if len(path) <= 2:
            return path

        smoothed = [path[0]]
        i = 0

        while i < len(path) - 1:
            j = len(path) - 1
            while j > i + 1:
                if self.is_line_clear(np.array(path[i]), np.array(path[j]), obstacles):
                    smoothed.append(path[j])
                    i = j
                    break
                j -= 1
            else:
                smoothed.append(path[i + 1])
                i += 1

        return smoothed

    def is_line_clear(self, start, end, obstacles):
        steps = 20
        for step in range(steps + 1):
            t = step / steps
            point = start + t * (end - start)

            for obs in obstacles:
                if np.linalg.norm(point - obs) < 1.5:
                    return False

        return True


class Sensor:
    """模拟传感器，用于检测未知障碍物"""

    def __init__(self, range=5.0, fov=120):
        self.range = range  # 传感器检测范围
        self.fov = fov  # 传感器视野角度（度）
        self.detected_obstacles = []  # 已检测到的障碍物

    def detect_obstacles(self, drone_position, drone_orientation, obstacles):
        """检测范围内的障碍物"""
        detected = []

        for obs in obstacles:
            # 计算障碍物相对于无人机的位置
            relative_pos = obs - drone_position

            # 计算距离
            distance = np.linalg.norm(relative_pos)

            if distance > self.range:
                continue

            # 计算角度（简化处理，只考虑水平方向）
            angle = np.degrees(np.arctan2(relative_pos[1], relative_pos[0]))

            # 检查是否在视野范围内
            if abs(angle - drone_orientation) <= self.fov / 2:
                detected.append(obs)

        # 记录新检测到的障碍物
        new_obstacles = [obs for obs in detected if
                         not any(np.array_equal(obs, known_obs) for known_obs in self.detected_obstacles)]

        if new_obstacles:
            logging.info(f"传感器检测到 {len(new_obstacles)} 个新障碍物")
            self.detected_obstacles.extend(new_obstacles)

        return new_obstacles


class Drone:
    def __init__(self, x=0, y=0, z=0, mass=1.0):
        self.position = np.array([x, y, z], dtype=float)
        self.velocity = np.array([0, 0, 0], dtype=float)
        self.acceleration = np.array([0, 0, 0], dtype=float)
        self.orientation = 0  # 无人机朝向（度）

        self.mass = mass
        self.max_thrust = 30.0
        self.gravity = np.array([0, 0, -9.81])

        self.pid_controller = PIDController(
            kp=np.array([2.0, 2.0, 6.0]),
            ki=np.array([0.1, 0.1, 0.1]),
            kd=np.array([1.0, 1.0, 3.0]),
            max_output=25.0
        )
        self.mpc_controller = MPCController()
        self.controller_type = "PID"

        self.trajectory = []
        self.control_forces = []
        self.step_count = 0
        self.gravity_compensation = np.array([0, 0, 9.81 * self.mass])

    def set_controller(self, controller_type):
        self.controller_type = controller_type
        logging.info(f"切换控制器到: {controller_type}")

    def update_orientation(self, target_direction):
        """更新无人机朝向，使其面向目标方向"""
        current_direction = self.velocity.copy()
        if np.linalg.norm(current_direction) < 0.1:
            current_direction = target_direction - self.position

        if np.linalg.norm(current_direction) > 0.1:
            # 计算目标方向的角度
            target_angle = np.degrees(np.arctan2(target_direction[1] - self.position[1],
                                                 target_direction[0] - self.position[0]))

            # 平滑转向
            angle_diff = target_angle - self.orientation
            if abs(angle_diff) > 180:
                angle_diff = angle_diff - 360 * np.sign(angle_diff)

            # 限制转向速度
            max_turn_rate = 30  # 度/秒
            turn_amount = np.clip(angle_diff, -max_turn_rate * 0.1, max_turn_rate * 0.1)
            self.orientation += turn_amount

            # 保持角度在0-360范围内
            self.orientation %= 360

    def update(self, target_position, dt=0.1):
        self.step_count += 1

        # 更新无人机朝向
        self.update_orientation(target_position)

        if self.controller_type == "PID":
            control_force = self.pid_controller.compute(target_position, self.position)
            control_force[2] += self.gravity_compensation[2]
        else:
            ref_trajectory = [target_position] * self.mpc_controller.horizon
            current_state = np.concatenate([self.position, self.velocity])
            control_force = self.mpc_controller.compute(current_state, ref_trajectory)
            control_force[2] += self.gravity_compensation[2]

        self.control_forces.append(control_force.copy())

        total_force = control_force + self.mass * self.gravity

        thrust_norm = np.linalg.norm(total_force)
        if thrust_norm > self.max_thrust:
            total_force = total_force / thrust_norm * self.max_thrust

        self.acceleration = total_force / self.mass
        self.velocity += self.acceleration * dt
        self.position += self.velocity * dt

        if self.position[2] < 0:
            self.position[2] = 0
            self.velocity[2] = 0

        self.trajectory.append(self.position.copy())

        if self.step_count % 10 == 0:
            error = target_position - self.position
            logging.info(
                f"步数 {self.step_count}: 位置={self.position}, 目标={target_position}, "
                f"误差={error}, 控制力={control_force}"
            )

        distance = np.linalg.norm(target_position - self.position)
        return distance < 0.5

    def get_position(self):
        return self.position.copy()

    def get_velocity(self):
        return self.velocity.copy()

    def get_trajectory(self):
        return np.array(self.trajectory)

    def get_control_forces(self):
        return np.array(self.control_forces)

    def get_orientation(self):
        return self.orientation


class NavigationSystem:
    def __init__(self):
        self.log_filename = setup_logging()
        self.data_logger = DataLogger()

        logging.info("初始化导航系统...")

        # 已知障碍物
        self.known_obstacles = [
            np.array([5, 5, 2]),
            np.array([8, 3, 3]),
            np.array([12, 8, 4]),
            np.array([15, 12, 5]),
            np.array([7, 10, 3]),
            np.array([10, 15, 4]),
            np.array([13, 5, 3]),
            np.array([18, 8, 4])
        ]

        # 未知障碍物（模拟环境中存在但无人机不知道的障碍物）
        self.unknown_obstacles = [
            np.array([14, 10, 3]),
            np.array([9, 12, 4]),
            np.array([16, 5, 3])
        ]

        self.start = np.array([0, 0, 1])
        self.goal = np.array([20, 15, 6])

        self.planner = AStarPlanner(grid_size=1.0)

        # 传感器
        self.sensor = Sensor(range=5.0, fov=120)

        # 创建无人机实例
        self.drone = Drone(
            x=self.start[0],
            y=self.start[1],
            z=self.start[2]
        )

        # 规划初始路径 - 现在可以在创建无人机后调用
        self.waypoints = self.plan_path(self.known_obstacles)

        self.current_waypoint_idx = 1
        self.mission_complete = False
        self.start_time = time.time()

        # 重规划相关变量
        self.replanning_count = 0
        self.last_replanning_time = 0
        self.replanning_cooldown = 2.0  # 重规划冷却时间（秒）

        logging.info(f"导航系统初始化完成，共{len(self.waypoints)}个航点")

    def plan_path(self, obstacles):
        """使用A*算法规划路径"""
        logging.info("正在规划路径...")
        path = self.planner.plan(self.drone.get_position(), self.goal, obstacles)

        if not path:
            logging.warning("无法找到路径! 使用直线路径")
            return [self.drone.get_position(), self.goal]

        logging.info(f"路径规划完成，共{len(path)}个航点")
        return path

    def check_for_obstacles(self):
        """使用传感器检测未知障碍物"""
        current_time = time.time()

        # 检查冷却时间
        if current_time - self.last_replanning_time < self.replanning_cooldown:
            return []

        # 检测障碍物
        new_obstacles = self.sensor.detect_obstacles(
            self.drone.get_position(),
            self.drone.get_orientation(),
            self.unknown_obstacles
        )

        return new_obstacles

    def dynamic_replanning(self, new_obstacles):
        """动态重规划路径"""
        current_time = time.time()

        # 检查冷却时间
        if current_time - self.last_replanning_time < self.replanning_cooldown:
            return False

        if new_obstacles:
            logging.info(f"检测到 {len(new_obstacles)} 个新障碍物，进行动态重规划...")

            # 将新障碍物添加到已知障碍物列表
            for obs in new_obstacles:
                if not any(np.array_equal(obs, known_obs) for known_obs in self.known_obstacles):
                    self.known_obstacles.append(obs)

            # 从当前位置重新规划路径
            new_waypoints = self.plan_path(self.known_obstacles)

            if len(new_waypoints) > 1:
                self.waypoints = new_waypoints
                self.current_waypoint_idx = 1  # 跳过当前位置（第0个点）
                self.replanning_count += 1
                self.last_replanning_time = current_time
                logging.info(f"动态重规划完成，新路径共{len(self.waypoints)}个航点")
                return True
            else:
                logging.warning("动态重规划失败，保持原路径")

        return False

    def update(self, dt=0.1):
        if self.mission_complete:
            return True

        # 获取当前目标航点
        target = self.waypoints[self.current_waypoint_idx]

        # 检测未知障碍物
        new_obstacles = self.check_for_obstacles()

        # 如果需要，进行动态重规划
        replanned = self.dynamic_replanning(new_obstacles)

        # 如果进行了重规划，更新目标点
        if replanned:
            target = self.waypoints[self.current_waypoint_idx]

        # 更新无人机位置
        reached = self.drone.update(target, dt)

        # 记录数据
        current_time = time.time() - self.start_time
        self.data_logger.log_data(
            current_time,
            self.drone.get_position(),
            self.drone.get_velocity(),
            target,
            self.drone.control_forces[-1] if self.drone.control_forces else np.zeros(3),
            self.replanning_count,
            len(new_obstacles)
        )

        # 如果到达当前航点，则转向下一个航点
        if reached:
            logging.info(f"到达航点 {self.current_waypoint_idx}: {target}")
            self.current_waypoint_idx += 1

            if self.current_waypoint_idx >= len(self.waypoints):
                logging.info("任务完成!")
                self.mission_complete = True
                return True

        return False

    def get_drone_position(self):
        return self.drone.get_position()

    def get_waypoints(self):
        return self.waypoints

    def get_trajectory(self):
        return self.drone.get_trajectory()

    def get_control_forces(self):
        return self.drone.get_control_forces()

    def get_obstacles(self):
        return self.known_obstacles + self.unknown_obstacles

    def get_known_obstacles(self):
        return self.known_obstacles

    def get_unknown_obstacles(self):
        return self.unknown_obstacles

    def set_controller(self, controller_type):
        self.drone.set_controller(controller_type)

    def print_summary(self):
        trajectory = self.get_trajectory()
        total_distance = 0
        for i in range(1, len(trajectory)):
            total_distance += np.linalg.norm(trajectory[i] - trajectory[i - 1])

        logging.info(f"任务摘要: 总飞行距离={total_distance:.2f}m, 航点数={len(self.waypoints)}")
        logging.info(f"重规划次数: {self.replanning_count}")
        logging.info(f"日志文件: {self.log_filename}")


class Simulation:
    def __init__(self, controller_type="PID"):
        self.nav_system = NavigationSystem()
        self.nav_system.set_controller(controller_type)

        self.fig = plt.figure(figsize=(15, 10))
        self.ax = self.fig.add_subplot(111, projection='3d')

        # 设置标题和标签，调整位置避免重叠
        self.ax.set_title(f'无人机导航模拟 - {controller_type}控制器 (带动态重规划)',
                          fontsize=14, pad=20)  # 增加pad使标题与图形有更多间距

        # 调整坐标轴标签位置
        self.ax.set_xlabel('X (m)', labelpad=15)  # 增加labelpad使标签远离坐标轴
        self.ax.set_ylabel('Y (m)', labelpad=15)
        self.ax.set_zlabel('Z (m)', labelpad=15)

        self.ax.set_xlim(-2, 25)
        self.ax.set_ylim(-2, 20)
        self.ax.set_zlim(0, 10)

        start = self.nav_system.start
        goal = self.nav_system.goal
        self.ax.scatter(start[0], start[1], start[2], c='green', marker='o', s=100, label='起点')
        self.ax.scatter(goal[0], goal[1], goal[2], c='red', marker='o', s=100, label='终点')

        # 绘制已知障碍物
        known_obstacles = self.nav_system.get_known_obstacles()
        for obs in known_obstacles:
            self.draw_cube(obs, 1.0, 'gray', alpha=0.7)

        # 绘制未知障碍物
        unknown_obstacles = self.nav_system.get_unknown_obstacles()
        for obs in unknown_obstacles:
            self.draw_cube(obs, 1.0, 'orange', alpha=0.3)

        # 绘制航点
        waypoints = self.nav_system.get_waypoints()
        wp_x = [wp[0] for wp in waypoints]
        wp_y = [wp[1] for wp in waypoints]
        wp_z = [wp[2] for wp in waypoints]
        self.waypoints_line, = self.ax.plot(wp_x, wp_y, wp_z, 'r--', alpha=0.5, label='规划路径')

        # 初始化无人机位置标记
        self.drone_marker, = self.ax.plot([], [], [], 'bo', markersize=8, label='无人机')

        # 添加无人机方向指示器
        self.drone_direction, = self.ax.plot([], [], [], 'b-', linewidth=2)

        # 初始化轨迹线
        self.trajectory_line, = self.ax.plot([], [], [], 'b-', alpha=0.7, linewidth=2, label='实际轨迹')

        # 初始化当前目标标记
        self.target_marker, = self.ax.plot([], [], [], 'rx', markersize=10, label='当前目标')

        # 添加传感器范围可视化
        self.sensor_range = None

        # 使用代理艺术家创建图例
        from matplotlib.patches import Rectangle
        known_obs_proxy = Rectangle((0, 0), 1, 1, fc='gray', alpha=0.7)
        unknown_obs_proxy = Rectangle((0, 0), 1, 1, fc='orange', alpha=0.3)

        # 手动创建图例
        handles, labels = self.ax.get_legend_handles_labels()
        handles.extend([known_obs_proxy, unknown_obs_proxy])
        labels.extend(['已知障碍物', '未知障碍物'])
        self.ax.legend(handles, labels)

        # 设置视角
        self.ax.view_init(elev=20, azim=45)

        # 添加信息文本框
        self.info_text = self.ax.text2D(0.02, 0.95, "", transform=self.ax.transAxes)

        # 添加重规划计数器
        self.replan_text = self.ax.text2D(0.02, 0.85, "", transform=self.ax.transAxes)

        self.animation = None
        self.frame_count = 0

    def draw_cube(self, center, size, color, alpha=0.5):
        """绘制立方体障碍物"""
        r = size / 2.0
        vertices = [
            [center[0] - r, center[1] - r, center[2] - r],
            [center[0] + r, center[1] - r, center[2] - r],
            [center[0] + r, center[1] + r, center[2] - r],
            [center[0] - r, center[1] + r, center[2] - r],
            [center[0] - r, center[1] - r, center[2] + r],
            [center[0] + r, center[1] - r, center[2] + r],
            [center[0] + r, center[1] + r, center[2] + r],
            [center[0] - r, center[1] + r, center[2] + r]
        ]

        faces = [
            [0, 1, 2, 3],
            [4, 5, 6, 7],
            [0, 1, 5, 4],
            [2, 3, 7, 6],
            [0, 3, 7, 4],
            [1, 2, 6, 5]
        ]

        poly_verts = []
        for face in faces:
            poly_verts.append([vertices[i] for i in face])

        collection = Poly3DCollection(poly_verts, facecolors=color, linewidths=0.5, edgecolors='k', alpha=alpha)
        self.ax.add_collection3d(collection)

    def update_sensor_display(self, drone_pos, orientation):
        """更新传感器范围显示"""
        if self.sensor_range:
            self.sensor_range.remove()

        # 创建传感器范围的圆锥体
        fov_rad = np.radians(self.nav_system.sensor.fov / 2)
        range_val = self.nav_system.sensor.range

        # 生成圆锥体点
        theta = np.linspace(0, 2 * np.pi, 20)
        z = np.linspace(0, range_val, 10)
        theta, z = np.meshgrid(theta, z)

        x = z * np.tan(fov_rad) * np.cos(theta)
        y = z * np.tan(fov_rad) * np.sin(theta)

        # 旋转到无人机方向
        angle_rad = np.radians(orientation)
        rot_x = x * np.cos(angle_rad) - y * np.sin(angle_rad)
        rot_y = x * np.sin(angle_rad) + y * np.cos(angle_rad)
        rot_z = z

        # 平移至无人机位置
        x_display = rot_x + drone_pos[0]
        y_display = rot_y + drone_pos[1]
        z_display = rot_z + drone_pos[2]

        # 绘制半透明传感器范围
        self.sensor_range = self.ax.plot_surface(
            x_display, y_display, z_display,
            color='cyan', alpha=0.1, linewidth=0, antialiased=True
        )

    def update_plot(self, frame):
        self.frame_count += 1

        complete = self.nav_system.update(dt=0.1)

        drone_pos = self.nav_system.get_drone_position()
        drone_orientation = self.nav_system.drone.get_orientation()

        if self.nav_system.current_waypoint_idx < len(self.nav_system.waypoints):
            target_pos = self.nav_system.waypoints[self.nav_system.current_waypoint_idx]
        else:
            target_pos = self.nav_system.waypoints[-1]

        # 更新无人机位置标记
        self.drone_marker.set_data([drone_pos[0]], [drone_pos[1]])
        self.drone_marker.set_3d_properties([drone_pos[2]])

        # 更新无人机方向指示器
        direction_length = 1.0
        angle_rad = np.radians(drone_orientation)
        end_x = drone_pos[0] + direction_length * np.cos(angle_rad)
        end_y = drone_pos[1] + direction_length * np.sin(angle_rad)
        self.drone_direction.set_data([drone_pos[0], end_x], [drone_pos[1], end_y])
        self.drone_direction.set_3d_properties([drone_pos[2], drone_pos[2]])

        # 更新目标位置标记
        self.target_marker.set_data([target_pos[0]], [target_pos[1]])
        self.target_marker.set_3d_properties([target_pos[2]])

        # 更新轨迹线
        trajectory = self.nav_system.get_trajectory()
        if len(trajectory) > 1:
            self.trajectory_line.set_data(trajectory[:, 0], trajectory[:, 1])
            self.trajectory_line.set_3d_properties(trajectory[:, 2])

        # 更新航点线（如果发生了重规划）
        waypoints = self.nav_system.get_waypoints()
        wp_x = [wp[0] for wp in waypoints]
        wp_y = [wp[1] for wp in waypoints]
        wp_z = [wp[2] for wp in waypoints]
        self.waypoints_line.set_data(wp_x, wp_y)
        self.waypoints_line.set_3d_properties(wp_z)

        # 更新传感器范围显示
        self.update_sensor_display(drone_pos, drone_orientation)

        # 更新信息文本
        info = f"帧: {self.frame_count}\n位置: {drone_pos}\n目标: {target_pos}\n误差: {np.linalg.norm(target_pos - drone_pos):.2f}m"
        if complete:
            info += "\n状态: 任务完成!"
        self.info_text.set_text(info)

        # 更新重规划信息
        replan_info = f"重规划次数: {self.nav_system.replanning_count}"
        self.replan_text.set_text(replan_info)

        # 如果任务完成，停止动画
        if complete:
            self.animation.event_source.stop()
            self.nav_system.print_summary()
            logging.info("模拟完成")

        return self.drone_marker, self.trajectory_line, self.target_marker, self.info_text, self.replan_text

    def run(self):
        logging.info("开始无人机导航模拟...")
        logging.info(f"控制器类型: {self.nav_system.drone.controller_type}")
        logging.info(f"起点: {self.nav_system.start}")
        logging.info(f"终点: {self.nav_system.goal}")
        logging.info(f"已知障碍物数量: {len(self.nav_system.get_known_obstacles())}")
        logging.info(f"未知障碍物数量: {len(self.nav_system.get_unknown_obstacles())}")

        self.animation = FuncAnimation(
            self.fig, self.update_plot, frames=1000,
            interval=50, blit=False, repeat=False
        )

        plt.show()


# 运行模拟
if __name__ == "__main__":
    sim = Simulation(controller_type="PID")
    sim.run()